/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package components;

import java.util.Iterator;

import ruter.simulator.Driver;
import ruter.simulator.Simulation;
import ruter.simulator.Vehicle;
import ruter.map.Geometry;
import ruter.map.MapComponent;
import ruter.network.Node;
import ruter.map.Map;
import ruter.network.UnavailableInformationException;

/**
 *
 * @author camilo
 */
public class RobotPilot extends Node implements Driver {

	private static final double MAX_ANGLE_RANGE = Math.PI * 1.5;
	private static final double SPEED_LIMIT = 8;
	private static final double ACELERATION = SPEED_LIMIT / 4;
	private static final double TURNING_ANGLE = Geometry.TAU;
	private static final double DISTANCE_TO_FIND_TARGET = 18;

	private Vehicle v;
	private Map m;
	private MapComponent t;
	public GaussianRandomMove objective;
	private Simulation simulation;
	private int id;
	private long timeToFind;
	private boolean useRules;

	public RobotPilot(Map map, MapComponent target, double frameDuration, Simulation simulation) {
		this.m = map;
		this.t = target;
		this.objective = new GaussianRandomMove(frameDuration * SPEED_LIMIT * 25);
		this.simulation = simulation;
		this.useRules = true;
	}

	public void setUseRules(boolean useRules) {
		this.useRules = useRules;
	}

	@Override
	public void drive(double frameDuration) {

		// Target
		if(t != null && Geometry.distance(v.x, v.y, t.getX(), t.getY()) < DISTANCE_TO_FIND_TARGET) {
			v.speed = 0;
			if(timeToFind == 0){
				timeToFind = simulation.getRealTime();
				v.removeMap();
			}
			return;
		}

		// Defining the objective
		if(useRules) {
			switch (connections.size()) {

			case 0:
//				objective.x = v.x;
//				objective.y = v.y;
				randomPosition();
				break;

			case 1:
				Node node = connections.keySet().iterator().next();
				if(node.connections.size() > 1) {
					if (connections.get(node) < 0.1) {
						try {
							objective.x = node.getX();
							objective.y = node.getY();
						} catch (UnavailableInformationException uie) {
						}
					} else {
						randomPosition();
					}
				} else {
					randomPosition();
				}
				break;

			case 2:
				Iterator<Node> it = connections.keySet().iterator();
				Node node1 = it.next();
				Node node2 = it.next();
				if (node1.connections.size() > 2 || node2.connections.size() > 2) {
					try {
						objective.x = (node1.getX() + node2.getX()) / 2;
						objective.y = (node1.getY() + node2.getY()) / 2;
					} catch (UnavailableInformationException uie) {
					}
				} else {
					randomPosition();
				}
				break;

			default:
				randomPosition();
				break;
			}
		} else {
			randomPosition();
		}

		// Check limits in the map
		if (objective.x < 0) {
			objective.x = -objective.x;
		} else if (objective.x > m.getXBounds()) {
			objective.x = m.getXBounds() - 20;
		}
		if (objective.y < 0) {
			objective.y = -objective.y;
		} else if (objective.y > m.getYBounds()) {
			objective.y = m.getYBounds() - 20;
		}

		// Calculating important values
		double distanceRemaining = Geometry.distance(v.x, v.y, objective.x, objective.y);

		// Auto-drive with one aim
		v.autoDrive(objective.x, objective.y, TURNING_ANGLE * frameDuration, MAX_ANGLE_RANGE);

		// Gauss-Markov speed control
		double acelerationNeeded = Math.pow(v.speed, 2) / (2 * distanceRemaining);
		if (acelerationNeeded > ACELERATION) {
			v.speed -= ACELERATION * frameDuration;
			if (v.speed < 0) {
				v.speed = 0;
			}
		} else {
			v.speed += ACELERATION * frameDuration;
			if (v.speed > SPEED_LIMIT) {
				v.speed = SPEED_LIMIT;
			}
		}
	}

	public long getTimeToFind() {
		return timeToFind;
	}

	private void randomPosition() {
		objective.generateRandomMove(v.x, v.y);
	}

	@Override
	public void obtainVehicle(Vehicle v) {
		this.v = v;
		this.objective.x = v.x + 1;
		this.objective.y = v.y + 1;
	}

	@Override
	public double getX() throws UnavailableInformationException {
		return v.x;
	}

	@Override
	public double getY() throws UnavailableInformationException {
		return v.y;
	}

	@Override
	public void setId(int id) {
		this.id = id;
	}

	@Override
	public int getId() {
		return id;
	}

}
